#include "StdAfx.h"
#include "KinectConnector.h"

namespace RecognizeEngine
{

	KinectConnector::KinectConnector()
		: m_pKinectSensor(nullptr)
		, m_pMultiSourceFrameReader(nullptr)
		, m_pBodies(nullptr)
	{
		if (FAILED(InitializeDefaultSensor()))
			throw std::runtime_error("initialize default sensor failed");
	}


	KinectConnector::~KinectConnector()
	{
		UninitializeBodies();
	}

	HRESULT KinectConnector::InitializeDefaultSensor()
	{
		HRESULT hr;

		hr = GetDefaultKinectSensor(&m_pKinectSensor);
		if (FAILED(hr))
		{
			return hr;
		}

		if (m_pKinectSensor)
		{
			hr = m_pKinectSensor->Open();
			if (FAILED(hr))
				return hr;

			hr = m_pKinectSensor->OpenMultiSourceFrameReader(FrameSourceTypes_Color | FrameSourceTypes_Body, &m_pMultiSourceFrameReader);
		}

		if (!m_pKinectSensor || FAILED(hr))
		{
			//SetStatusMessage(L"No ready Kinect found!", 10000, true);
			return E_FAIL;
		}

		CComPtr<IBodyFrameSource> bodyFrameSource;
		hr = m_pKinectSensor->get_BodyFrameSource(&bodyFrameSource);
		if (FAILED(hr))
		{
			return E_FAIL;
		}

		INT32 bodyCount;
		hr = bodyFrameSource->get_BodyCount(&bodyCount);
		m_nBodyCount = bodyCount;
		InitializeBodies();

		try
		{
			for (UINT i = 0; i != m_nBodyCount; i++)
			{
				std::unique_ptr<GestureDetector> detector(new GestureDetector(m_pKinectSensor));
				m_vecGestureDetectors.push_back(std::move(detector));
			}
		}
		catch (std::exception& e)
		{
			printf("%s\n", e.what());
			return E_FAIL;
		}

		return hr;
	}

	void KinectConnector::Update()
	{
		if (m_pMultiSourceFrameReader == nullptr)
			return;

		HRESULT hr;

		CComPtr<IMultiSourceFrame> frame;
		hr = m_pMultiSourceFrameReader->AcquireLatestFrame(&frame);
		if (FAILED(hr))
			return;

		CComPtr<IBodyFrameReference> body_ref;
		hr = frame->get_BodyFrameReference(&body_ref);
		if (SUCCEEDED(hr)) 
		{
			CComPtr<IBodyFrame> bodyFrame;
			hr = body_ref->AcquireFrame(&bodyFrame);
			if (SUCCEEDED(hr) && bodyFrame) 
			{
				hr = bodyFrame->GetAndRefreshBodyData(m_nBodyCount, m_pBodies);
				if (SUCCEEDED(hr))
				{
					for (UINT i = 0; i != m_nBodyCount; i++)
					{
						auto& detector = m_vecGestureDetectors[i];

						UINT64 trackingId;
						m_pBodies[i]->get_TrackingId(&trackingId);
						if (trackingId != detector->GetTrackingId())
						{
							detector->SetTrackingId(trackingId);
							detector->SetPaused(trackingId == 0);
						}

						detector->Update(m_pBodies[i]);
					}
				}
			}
		}

		CComPtr<IColorFrameReference> color_ref;
		hr = frame->get_ColorFrameReference(&color_ref);
		if (SUCCEEDED(hr)) 
		{
			CComPtr<IColorFrame> colorFrame;
			hr = color_ref->AcquireFrame(&colorFrame);
			if (SUCCEEDED(hr))
			{
				hr = m_imageSource.Update(colorFrame);
			}
		}
	}

	void KinectConnector::InitializeBodies()
	{
		if (m_pBodies == nullptr)
		{
			m_pBodies = new IBody*[m_nBodyCount];

			for (UINT i = 0; i != m_nBodyCount; i++) {
				m_pBodies[i] = nullptr;
			}
		}
	}

	void KinectConnector::UninitializeBodies()
	{
		if (m_pBodies != nullptr)
		{
			for (UINT i = 0; i != m_nBodyCount; i++) {
				SafeRelease(m_pBodies[i]);
			}
		}
	}

}
